#include <SPI.h>

#include <Pixy.h>

Pixy pixy;

int L_PMW =6;

int L_INA =8;

int L_INB =7;

int L_CS=9;

int L_EN=10;

int R_PMW =1;

int R_INA =3;

int R_INB =2;

int R_CS=4;

int R_EN=5;

int posX;

int posY;

void setup()

{

pinMode(L_PMW, OUTPUT);

pinMode(L_INA, OUTPUT);

pinMode(L_INB, OUTPUT);

pinMode(L_CS, OUTPUT);

pinMode(L_EN, OUTPUT);

pinMode(R_PMW, OUTPUT);

pinMode(R_INA, OUTPUT);

pinMode(R_INB, OUTPUT);

pinMode(R_CS, OUTPUT);

pinMode(R_EN, OUTPUT);

digitalWrite(L_CS, HIGH);

digitalWrite(L_EN, HIGH);

digitalWrite(R_CS, HIGH);

digitalWrite(R_EN, HIGH);

  Serial.begin(9600);

//  Serial.print("Starting...\n");

  pixy.init();

}

void loop()

{

  uint16_t type;

  static int i = 0;

  int j;

  int16_t signature;

  int16_t angle;

  uint16_t blocks;           

  blocks = pixy.getBlocks(); 

    int POSX;

    int POSW;

  if (blocks)                

  {

//        Serial.print("Sign =");

//        signature = (pixy.blocks[j].signature);

//        Serial.print(signature,OCT);

       // Serial.print("\t");

//        Serial.print("Loc X =");

//        Serial.print(pixy.blocks[j].x);

        POSX=(pixy.blocks[j].x);

       // Serial.print("\t");

//        Serial.print("Loc Y =");

//        Serial.print(pixy.blocks[j].y);

//        Serial.print("\t");

//        

//        Serial.print("width =");

//        Serial.print(pixy.blocks[j].width);

        POSW=(pixy.blocks[j].width);

//        Serial.println("\t");

//        

//        Serial.print("Height =");

//        Serial.print(pixy.blocks[j].height);

//        Serial.print("\t");

//        

//        Serial.print("angle =");

//        Serial.print(angle);

//        Serial.print("\t");

//        

//        pixy.blocks[j].print(); //<<<----- This print all one Line

 if(POSW>80)

{

  ST();

  }

else if(POSX>150&&POSX<180)

{

  GO();

  }

 else if(POSX<150)

{

 LEFT();

} 

else if(POSX>180)

{

 RIGH();

}  

else

{

ST();  

}

    }

  }

  void GO() 

{

digitalWrite(R_PMW, HIGH);///ความเร็ว

digitalWrite(R_INA, HIGH);///ทิศทางการหมุน

digitalWrite(R_INB, LOW);//////ทิศทางการหมุน

digitalWrite(L_PMW, HIGH);///ความเร็ว

digitalWrite(L_INA, HIGH);///ทิศทางการหมุน

digitalWrite(L_INB, LOW);//////ทิศทางการหมุน

delay(100);

digitalWrite(R_PMW, LOW);///ความเร็ว

digitalWrite(R_INA, LOW);///ทิศทางการหมุน

digitalWrite(R_INB, LOW);//////ทิศทางการหมุน

digitalWrite(L_PMW, LOW);///ความเร็ว

digitalWrite(L_INA, LOW);///ทิศทางการหมุน

digitalWrite(L_INB, LOW);//////ทิศทางการหมุน

}

void BACK() 

{

digitalWrite(R_PMW, HIGH);///ความเร็ว

digitalWrite(R_INA, LOW);///ทิศทางการหมุน

digitalWrite(R_INB, HIGH);//////ทิศทางการหมุน

digitalWrite(L_PMW, HIGH);///ความเร็ว

digitalWrite(L_INA, LOW);///ทิศทางการหมุน

digitalWrite(L_INB, HIGH);//////ทิศทางการหมุน

}

void RIGH() 

{

digitalWrite(R_PMW, HIGH);///ความเร็ว

digitalWrite(R_INA, LOW);///ทิศทางการหมุน

digitalWrite(R_INB, HIGH);//////ทิศทางการหมุน

digitalWrite(L_PMW, HIGH);///ความเร็ว

digitalWrite(L_INA, HIGH);///ทิศทางการหมุน

digitalWrite(L_INB, LOW);//////ทิศทางการหมุน

delay(100);

digitalWrite(R_PMW, LOW);///ความเร็ว

digitalWrite(R_INA, LOW);///ทิศทางการหมุน

digitalWrite(R_INB, LOW);//////ทิศทางการหมุน

digitalWrite(L_PMW, LOW);///ความเร็ว

digitalWrite(L_INA, LOW);///ทิศทางการหมุน

digitalWrite(L_INB, LOW);//////ทิศทางการหมุน

}

void LEFT() 

{

digitalWrite(R_PMW, HIGH);///ความเร็ว

digitalWrite(R_INA, HIGH);///ทิศทางการหมุน

digitalWrite(R_INB, LOW);//////ทิศทางการหมุน

digitalWrite(L_PMW, HIGH);///ความเร็ว

digitalWrite(L_INA, LOW);///ทิศทางการหมุน

digitalWrite(L_INB, HIGH);//////ทิศทางการหมุน

delay(100);

digitalWrite(R_PMW, LOW);///ความเร็ว

digitalWrite(R_INA, LOW);///ทิศทางการหมุน

digitalWrite(R_INB, LOW);//////ทิศทางการหมุน

digitalWrite(L_PMW, LOW);///ความเร็ว

digitalWrite(L_INA, LOW);///ทิศทางการหมุน

digitalWrite(L_INB, LOW);//////ทิศทางการหมุน

}

void ST() 

{

digitalWrite(R_PMW, LOW);///ความเร็ว

digitalWrite(R_INA, LOW);///ทิศทางการหมุน

digitalWrite(R_INB, LOW);//////ทิศทางการหมุน

digitalWrite(L_PMW, LOW);///ความเร็ว

digitalWrite(L_INA, LOW);///ทิศทางการหมุน

digitalWrite(L_INB, LOW);//////ทิศทางการหมุน

}